#include <arpa/inet.h>
#include <assert.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "rtp.h"

#define H264_FILE_NAME "test.h264"
#define SERVER_PORT 8554
#define SERVER_RTP_PORT 55532
#define SERVER_RTCP_PORT 55533
#define BUF_MAX_SIZE (1024 * 1024)

static int createTcpSocket() {
  int sockfd;
  int on = 1;

  sockfd = socket(AF_INET, SOCK_STREAM, 0);
  if (sockfd < 0) return -1;

  setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (const char*)&on, sizeof(on));

  return sockfd;
}

static int createUdpSocket() {
  int sockfd;
  int on = 1;

  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  if (sockfd < 0) return -1;

  setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (const char*)&on, sizeof(on));

  return sockfd;
}

static int bindSocketAddr(int sockfd, const char* ip, int port) {
  struct sockaddr_in addr;

  addr.sin_family = AF_INET;
  addr.sin_port = htons(port);
  addr.sin_addr.s_addr = inet_addr(ip);

  if (bind(sockfd, (struct sockaddr*)&addr, sizeof(struct sockaddr)) < 0)
    return -1;

  return 0;
}

static int acceptClient(int sockfd, char* ip, int* port) {
  int clientfd;
  socklen_t len = 0;
  struct sockaddr_in addr;

  memset(&addr, 0, sizeof(addr));
  len = sizeof(addr);

  clientfd = accept(sockfd, (struct sockaddr*)&addr, &len);
  if (clientfd < 0) return -1;

  strcpy(ip, inet_ntoa(addr.sin_addr));
  *port = ntohs(addr.sin_port);

  return clientfd;
}

static inline int startCode3(char* buf) {
  if (buf[0] == 0 && buf[1] == 0 && buf[2] == 1)
    return 1;
  else
    return 0;
}

static inline int startCode4(char* buf) {
  if (buf[0] == 0 && buf[1] == 0 && buf[2] == 0 && buf[3] == 1)
    return 1;
  else
    return 0;
}

static char* findNextStartCode(char* buf, int len) {
  int i;

  if (len < 3) return NULL;

  for (i = 0; i < len - 3; ++i) {
    if (startCode3(buf) || startCode4(buf)) return buf;

    ++buf;
  }

  if (startCode3(buf)) return buf;

  return NULL;
}

static int getFrameFromH264File(FILE* file, char* frame, int size) {
  int rSize, frameSize;
  char* nextStartCode;

  rSize = fread(frame, 1, size, file);
  if (!startCode3(frame) && !startCode4(frame)) return -1;

  nextStartCode = findNextStartCode(frame + 3, rSize - 3);
  if (!nextStartCode) {
    // lseek(fd, 0, SEEK_SET);
    // frameSize = rSize;
    return -1;
  } else {
    frameSize = (nextStartCode - frame);
    fseek(file, frameSize - rSize, SEEK_CUR);
  }

  return frameSize;
}

static int rtpSendH264Frame(int socket, const char* ip, int16_t port,
                            struct RtpPacket* rtpPacket, uint8_t* frame,
                            uint32_t frameSize) {
  uint8_t naluType;  // nalu第一个字节
  int sendBytes = 0;
  int ret;
  rtpPacket->rtpHeader.marker = 0;
  naluType = frame[0];
  if (frameSize <= RTP_MAX_PKT_SIZE)  // nalu长度小于最大包场：单一NALU单元模式
  {
    /*
     *   0 1 2 3 4 5 6 7 8 9
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |F|NRI|  Type   | a single NAL unit ... |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     */
    // 如果是SPS、PPS就不需要加时间戳
    if ((naluType & 0x1F) == 7 || (naluType & 0x1F) == 8) {
      // NOTE, 这里去除重复的sps、pps
      if (rtpPacket->offset > 41) {
        return 0;
      }
    } else {
      // NOTE, 单独帧包
      rtpPacket->rtpHeader.marker = 1;
    }
    printf("0x%02x length %d\n", naluType, frameSize);
    rtpWriteData(rtpPacket, frame, frameSize);
    if ((naluType & 0x1F) == 7 || (naluType & 0x1F) == 8) {
      goto out;
    }
    ret = rtpSendPacket(socket, ip, port, rtpPacket);

    sendBytes += ret;
  } else {
    // nalu长度小于最大包场：分片模式
    /*
     *  0                   1                   2
     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * | FU indicator  |   FU header   |   FU payload   ...  |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     */

    /*
     *     FU Indicator
     *    0 1 2 3 4 5 6 7
     *   +-+-+-+-+-+-+-+-+
     *   |F|NRI|  Type   |
     *   +---------------+
     */

    /*
     *      FU Header
     *    0 1 2 3 4 5 6 7
     *   +-+-+-+-+-+-+-+-+
     *   |S|E|R|  Type   |
     *   +---------------+
     */
    frameSize--;
    int pktNum = frameSize / RTP_MAX_PKT_SIZE;         // 有几个完整的包
    int remainPktSize = frameSize % RTP_MAX_PKT_SIZE;  //
    if (remainPktSize == 0) {
      // NOTE, 如果是RTP_MAX_PKT_SIZE倍数包，把最后一包当成结束包
      remainPktSize = RTP_MAX_PKT_SIZE;
      pktNum--;
    }
    int i, pos = 1;
    /* 发送完整的包 */
    printf("0x%02x length %d\n", naluType, frameSize);
    frameSize -= pos;
    uint8_t ptr[RTP_MAX_PKT_SIZE + 4] = {0};
    for (i = 0; i < pktNum; i++) {
      ptr[0] = (naluType & 0x60) | 28;
      ptr[1] = naluType & 0x1F;

      if (i == 0)        // 第一包数据
        ptr[1] |= 0x80;  // start

      memcpy(ptr + 2, frame + pos, RTP_MAX_PKT_SIZE);
      rtpWriteData(rtpPacket, ptr, RTP_MAX_PKT_SIZE + 2);
      ret = rtpSendPacket(socket, ip, port, rtpPacket);
      if (ret < 0) return -1;

      sendBytes += ret;
      pos += RTP_MAX_PKT_SIZE;
    }

    /* 发送剩余的数据 */
    if (remainPktSize > 0) {
      ptr[0] = (naluType & 0x60) | 28;
      ptr[1] = naluType & 0x1F;
      ptr[1] |= 0x40;  // end
      // NOTE, 结束帧包
      rtpPacket->rtpHeader.marker = 1;
      memcpy(ptr + 2, frame + pos, remainPktSize + 2);
      rtpWriteData(rtpPacket, ptr, remainPktSize + 2);
      ret = rtpSendPacket(socket, ip, port, rtpPacket);
      if (ret < 0) return -1;

      sendBytes += ret;
    }
  }

out:

  return sendBytes;
}

static char* getLineFromBuf(char* buf, char* line) {
  while (*buf != '\n') {
    *line = *buf;
    line++;
    buf++;
  }

  *line = '\n';
  ++line;
  *line = '\0';

  ++buf;
  return buf;
}

static int handleCmd_OPTIONS(char* result, int cseq) {
  sprintf(result,
          "RTSP/1.0 200 OK\r\n"
          "CSeq: %d\r\n"
          "Public: OPTIONS, DESCRIBE, SETUP, PLAY\r\n"
          "\r\n",
          cseq);

  return 0;
}

static int handleCmd_DESCRIBE(char* result, int cseq, char* url) {
  char sdp[2000];
  char localIp[100];

  sscanf(url, "rtsp://%[^:]:", localIp);
  const char* fmtp =
      "packetization-mode=1;profile-level-id=42801F;sprop-parameter-sets="
      "Z0KAH5ZSAKALdJQEBAUAAAMAAQAAAwAyhA==,aMuNSA==";

  sprintf(sdp,
          "v=0\r\n"
          "o=- %ld 1 IN IP4 %s\r\n"
          "s=RTSP Session/2.0\r\n"
          "c=IN IP4 0.0.0.0\r\n"
          "t=0 0\r\n"
          "a=control:*\r\n"
          "a=range:npt=0-600.000000\r\n"
          "a=packetization-supported:DH\r\n"
          "m=video 0 RTP/AVP 96\r\n"
          "a=control:trackID=0\r\n"
          "a=rtpmap:96 H264/90000\r\n"
          "a=fmtp:96 %s\r\n"
          "a=recvonly\r\n",
          time(NULL), localIp, fmtp);

  sprintf(result,
          "RTSP/1.0 200 OK\r\nCSeq: %d\r\n"
          "Content-Base: %s\r\n"
          "Content-Type: application/sdp\r\n"
          "Content-Length: %lu\r\n\r\n"
          "%s",
          cseq, url, strlen(sdp), sdp);

  return 0;
}

static int handleCmd_SETUP(char* result, int cseq, int clientRtpPort) {
  sprintf(result,
          "RTSP/1.0 200 OK\r\n"
          "CSeq: %d\r\n"
          "Transport: RTP/AVP/TCP;unicast;interleaved=0-1;ssrc=2bf38cfa\r\n"
          "Session: 66334873;timeout=60\r\n"
          "\r\n",
          cseq);

  return 0;
}

static int handleCmd_PLAY(char* result, int rtptime, uint32_t cseq) {
  sprintf(result,
          "RTSP/1.0 200 OK\r\n"
          "CSeq: %d\r\n"
          "Range: npt=0.000000-120.000000\r\n"
          "RTP-Info: url=trackID=0;seq=0;rtptime=%u;ssrc=2bf38cfa\r\n"
          "Session: 66334873;timeout=60\r\n\r\n",
          rtptime, cseq);

  return 0;
}

static void doClient(int clientSockfd, const char* clientIP, int clientPort) {
  char method[40];
  char url[100];
  char version[40];
  int cseq;
  int clientRtpPort, clientRtcpPort;
  char* bufPtr;
  char* rBuf = malloc(BUF_MAX_SIZE);
  char* sBuf = malloc(BUF_MAX_SIZE);
  char line[400];

  while (1) {
    int recvLen;

    recvLen = recv(clientSockfd, rBuf, BUF_MAX_SIZE, 0);
    if (recvLen <= 0) {
      printf("failed to recv %d\n", recvLen);
      goto out;
    }

    rBuf[recvLen] = '\0';
    printf("---------------C->S--------------\n");
    printf("%s", rBuf);

    /* 解析方法 */
    bufPtr = getLineFromBuf(rBuf, line);
    if (sscanf(line, "%s %s %s\r\n", method, url, version) != 3) {
      printf("parse err\n");
      goto out;
    }

    /* 解析序列号 */
    bufPtr = getLineFromBuf(bufPtr, line);
    if (sscanf(line, "CSeq: %d\r\n", &cseq) != 1) {
      printf("parse err\n");
      goto out;
    }

    /* 如果是SETUP，那么就再解析client_port */
    if (!strcmp(method, "SETUP")) {
      while (1) {
        bufPtr = getLineFromBuf(bufPtr, line);
        if (!strncmp(line, "Transport:", strlen("Transport:"))) {
          sscanf(line, "Transport: RTP/AVP;unicast;client_port=%d-%d\r\n",
                 &clientRtpPort, &clientRtcpPort);
          break;
        }
      }
    }
    uint32_t rtptime = time(NULL) / 9000;
    if (!strcmp(method, "OPTIONS")) {
      if (handleCmd_OPTIONS(sBuf, cseq)) {
        printf("failed to handle options\n");
        goto out;
      }
    } else if (!strcmp(method, "DESCRIBE")) {
      if (handleCmd_DESCRIBE(sBuf, cseq, url)) {
        printf("failed to handle describe\n");
        goto out;
      }
    } else if (!strcmp(method, "SETUP")) {
      if (handleCmd_SETUP(sBuf, cseq, clientRtpPort)) {
        printf("failed to handle setup\n");
        goto out;
      }
    } else if (!strcmp(method, "PLAY")) {
      if (handleCmd_PLAY(sBuf, cseq, rtptime)) {
        printf("failed to handle play\n");
        goto out;
      }
    } else {
      printf("failed to handle %s\n", method);
      goto out;
    }

    printf("---------------S->C--------------\n");
    printf("%s", sBuf);
    send(clientSockfd, sBuf, strlen(sBuf), 0);

    /* 开始播放，发送RTP包 */
    if (!strcmp(method, "PLAY")) {
      int frameSize, startCode;
      char* frame = malloc(500000);
      struct RtpPacket rtpPacket;
      FILE* file = fopen(H264_FILE_NAME, "r");
      assert(file != NULL);
      rtpHeaderInit(&rtpPacket, 0, 0, 0, RTP_VESION, RTP_PAYLOAD_TYPE_H264, 0,
                    46635, rtptime, 0x2bf38cfa);

      printf("start play\n");
      printf("client ip:%s\n", clientIP);
      printf("client port:%d\n", clientRtpPort);

      while (1) {
        memset(frame, 0, 500000);
        frameSize = getFrameFromH264File(file, frame, 500000);
        if (frameSize < 0) {
          break;
        }

        if (startCode3(frame))
          startCode = 3;
        else
          startCode = 4;

        frameSize -= startCode;
        uint8_t* data = (uint8_t*)(frame + startCode);
        int ret = rtpSendH264Frame(clientSockfd, clientIP, clientRtpPort,
                                   &rtpPacket, data, frameSize);
        if (ret < 0) {
          break;
        }
        if (ret > 0) {
          rtpPacket.rtpHeader.timestamp += (90000 / 25);
          usleep(1000 * 1000 / 25);
        }
      }
      free(frame);
      free(rtpPacket.payload);
      fclose(file);
      goto out;
    }
  }
out:
  printf("finish\n");
  close(clientSockfd);
  free(rBuf);
  free(sBuf);
}

int main(int argc, char* argv[]) {
  int serverSockfd;
  // int serverRtpSockfd, serverRtcpSockfd;

  serverSockfd = createTcpSocket();
  if (serverSockfd < 0) {
    printf("failed to create tcp socket\n");
    return -1;
  }

  if (bindSocketAddr(serverSockfd, "0.0.0.0", SERVER_PORT) < 0) {
    printf("failed to bind addr\n");
    return -1;
  }

  if (listen(serverSockfd, 10) < 0) {
    printf("failed to listen\n");
    return -1;
  }

  // serverRtpSockfd = createUdpSocket();
  // serverRtcpSockfd = createUdpSocket();
  // if (serverRtpSockfd < 0 || serverRtcpSockfd < 0) {
  //   printf("failed to create udp socket\n");
  //   return -1;
  // }

  // if (bindSocketAddr(serverRtpSockfd, "0.0.0.0", SERVER_RTP_PORT) < 0 ||
  //     bindSocketAddr(serverRtcpSockfd, "0.0.0.0", SERVER_RTCP_PORT) < 0) {
  //   printf("failed to bind addr\n");
  //   return -1;
  // }

  printf("rtsp://127.0.0.1:%d\n", SERVER_PORT);

  while (1) {
    int clientSockfd;
    char clientIp[40];
    int clientPort;

    clientSockfd = acceptClient(serverSockfd, clientIp, &clientPort);
    if (clientSockfd < 0) {
      printf("failed to accept client\n");
      return -1;
    }

    printf("accept client;client ip:%s,client port:%d\n", clientIp, clientPort);

    doClient(clientSockfd, clientIp, clientPort);
  }

  return 0;
}
